# -*- coding: utf-8 -*-

###############################################################################
# Copyright [2010] [Roberto Carrasco - rcarrasco@talentus.cl]
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
###############################################################################

"""Skytraq Access Modules"""

import serial
import time
import threading
import operator

import Utils
import SimpleByteQueue
import NmeaResponse
import BinaryResponse

# Input System Messages
CMD_SYSTEM_RESTART = 0x01
CMD_QUERY_SOFTWARE_VERSION = 0x02
CMD_QUERY_SOFTWARE_CRC = 0x03
CMD_SET_FACTORY_DEFAULTS = 0x04
CMD_CONFIGURE_SERIAL_PORT = 0x05
#RESERVED = 0x06
#RESERVED = 0x07
CMD_CONFIGURE_NMEA=0x08
CMD_CONFIGURE_OUTPUT_MESSAGE_FORMAT = 0x09
CMD_CONFIGURE_POWER_MODE = 0x0c
CMD_CONFIGURE_POSITION_UPDATE_RATE = 0x0e
CMD_QUERY_POSITION_UPDATE_RATE = 0x10

# Input GPS Messages
CMD_CONFIGURE_DATUM = 0x29
CMD_QUERY_DATUM = 0x2d
CMD_GET_EPHEMERIS = 0x30
CMD_SET_EPHEMERIS = 0x31
CMD_CONFIGURE_WAAS = 0x37
CMD_QUERY_WAAS_STATUS = 0x38
CMD_CONFIGURE_POSITION_PINNING = 0x39
CMD_QUERY_POSITION_PINNING = 0x3a
CMD_CONFIGURE_POSITION_PINNING_PARAMETERS = 0x3b
CMD_CONFIGURE_NAVIGATION_MODE = 0x3c
CMD_QUERY_NAVIGATION_MODE = 0x3d
CMD_CONFIGURE_ONEPPS_MODE = 0x3e
CMD_QUERY_ONEPPS_MODE = 0x3f

# Data Logging Binary Messages
CMD_LOG_STATUS_CONTROL = 0x17
CMD_LOG_CONFIGURE_CONTROL = 0x18
CMD_LOG_CLEAR_CONTROL = 0x19
CMD_LOG_SECTOR_READ_CONTROL = 0x1b      # not in AN0008_v.1.4.11
CMD_LOG_READ_BATCH_CONTROL = 0x1d

# Output System Messages
RSP_SOFTWARE_VERSION = 0x80
RSP_SOFTWARE_CRC = 0x81
#RESERVED = 0x82
RSP_ACK= 0x83
RSP_NACK = 0x84
RSP_POSITION_UPDATE_RATE = 0x86

# Output GPS Messages
RSP_LOG_STATUS_OUTPUT = 0x94
RSP_GPS_DATUM = 0xae
RSP_GPS_EPHEMERIS_DATA = 0xb1
RSP_GPS_WAAS_STATUS = 0xb3
RSP_GPS_POSITION_PINNING_STATUS = 0xb4
RSP_GPS_NAVIGATION_MODE = 0xb5
RSP_GPS_ONEPPS_MODE = 0xb6

# i need some code to wait for sectors
RSP_SECTOR = 0xFF

# param start_mode
#   CMD_SYSTEM_RESTART
START_MODE_NOCHANGE = 0x00
START_MODE_HOTSTART = 0x01
START_MODE_WARMSTART = 0x02
START_MODE_COLDSTART = 0x03

# param software_type
#   CMD_QUERY_SOFTWARE_VERSION
#   CMD_QUERY_SOFTWARE_CRC
SOFTWARE_TYPE_RESERVED = 0x00
SOFTWARE_TYPE_SYSTEM_CODE = 0x01

# param attribute
#   CMD_CONFIGURE_SERIAL_PORT
#   CMD_CONFIGURE_NMEA
#   CMD_CONFIGURE_POWER_MODE
#   CMD_CONFIGURE_POSITION_UPDATE_RATE
ATTRIBUTE_UPDATE_TO_SRAM = 0x00
ATTRIBUTE_UPDATE_TO_SRAM_AND_FLASH = 0x01
ATTRIBUTE_TEMPORARY_ENABLED = 0x02            # only for CMD_CONFIGURE_POWER_MODE

# param type
#   CMD_CONFIGURE_OUTPUT_MESSAGE_FORMAT
TYPE_NO_OUTPUT = 0x00
TYPE_NMEA_MESSAGE = 0x01
TYPE_BINARY_MESSAGE = 0x02

# param mode
#   CMD_CONFIGURE_POWER_MODE
MODE_DISABLED = 0x00
MODE_ENABLED = 0x01

# param rate
#   CMD_CONFIGURE_POSITION_UPDATE_RATE
RATE_1 = 1
RATE_2 = 2
RATE_4 = 3
RATE_5 = 4
RATE_8 = 5
RATE_10 = 10

class Skytraq:
    def __init__(self):
        self.serialPort = None
        self.serialData = SimpleByteQueue.SimpleByteQueue(1024)
        self.mutex = threading.Lock()
        self.abort = threading.Event()

    def connect(self, device, speed, timeout, nmea_observer):
        self.mutex.acquire()
        retval = False
        if self.serialPort == None:
            try:
                self.serialPort = serial.Serial(port = device, baudrate = speed, timeout = timeout)
                self.serialPort.open()
                self.serialData.empty()
                self.timeout = timeout
                self.nmea_observer = nmea_observer
                self.shared = None
                self.abort.clear()
                self.tDispatcher = threading.Thread(target = self._dispatcher)
                self.tDispatcher.start()
                while not self.tDispatcher.is_alive():
                    pass
                self.tPortReader = threading.Thread(target = self._portReader)
                self.tPortReader.start()
                while not self.tPortReader.is_alive():
                    pass
                retval=True
            except serial.SerialException:
                pass
        self.mutex.release()
        return retval

    def disconnect(self):
        self.mutex.acquire()
        if self.serialPort != None:
            self.abort.set()
            while self.tPortReader.is_alive():
                pass
            while self.tDispatcher.is_alive():
                pass
            self.serialPort.close()
            self.serialPort = None
        self.mutex.release()

    def sendSystemRestart(self, start_mode):
        if start_mode not in (START_MODE_NOCHANGE, START_MODE_HOTSTART, START_MODE_WARMSTART, START_MODE_COLDSTART):
            start_mode = START_MODE_NOCHANGE

        cmd = bytearray(15)
        cmd[0] = CMD_SYSTEM_RESTART
        cmd[1] = start_mode
        cmd[2] = 0x07 # año hi  07d0=2010
        cmd[3] = 0xda # año low 07d0=2010
        cmd[4] = 0x01 # mes UTC
        cmd[5] = 0x01 # día UTC
        cmd[6] = 0x00 # hora UTC
        cmd[7] = 0x00 # minutos UTC
        cmd[8] = 0x00 # segundos UTC
        cmd[9] = 0x00 # latitud hi
        cmd[10]= 0x00 # latitud lo
        cmd[11]= 0x00 # longitud hi
        cmd[12]= 0x00 # longitud lo
        cmd[13]= 0x00 # altitud hi
        cmd[14]= 0x00 # altitud lo
        return self._sendRequest(cmd)

    def sendQuerySoftwareVersion(self, software_type):
        if software_type not in (SOFTWARE_TYPE_RESERVED, SOFTWARE_TYPE_SYSTEM_CODE):
            software_type = SOFTWARE_TYPE_SYSTEM_CODE

        cmd = bytearray(2)
        cmd[0] = CMD_QUERY_SOFTWARE_VERSION
        cmd[1] = software_type
        return self._sendRequest(cmd, RSP_SOFTWARE_VERSION)

    def sendQuerySoftwareCRC(self, software_type):
        if software_type not in (SOFTWARE_TYPE_RESERVED, SOFTWARE_TYPE_SYSTEM_CODE):
            software_type = SOFTWARE_TYPE_SYSTEM_CODE

        cmd = bytearray(2)
        cmd[0] = CMD_QUERY_SOFTWARE_CRC
        cmd[1] = software_type
        return self._sendRequest(cmd, RSP_SOFTWARE_CRC)

    def sendSetFactoryDefaults(self, reboot):
        cmd = bytearray(2)
        cmd[0] = CMD_SET_FACTORY_DEFAULTS
        cmd[1] = reboot & 0x01
        return self._sendRequest(cmd)

    def sendConfigureSerialPort(self, baud, attribute):
        bauds = {4800:0x00, 9600:0x01, 19200:0x02, 38400:0x03, 57600:0x04, 115200:0x05}
        if baud in bauds:
            baud = bauds[baud]
        else:
            baud = bauds[38400]

        if attribute not in (ATTRIBUTE_UPDATE_TO_SRAM, ATTRIBUTE_UPDATE_TO_SRAM_AND_FLASH):
            attribute = ATTRIBUTE_UPDATE_TO_SRAM

        cmd = bytearray(4)
        cmd[0] = CMD_CONFIGURE_SERIAL_PORT
        cmd[1] = 0x00
        cmd[2] = baud
        cmd[3] = attribute
        return self._sendRequest(cmd)

    def sendConfigureNmea(self, gga, gsa, gsv, gll, rmc, vtg, zda, attribute):
        if attribute not in(ATTRIBUTE_UPDATE_TO_SRAM, ATTRIBUTE_UPDATE_TO_SRAM_AND_FLASH):
            attribute = ATTRIBUTE_UPDATE_TO_SRAM

        cmd = bytearray(9)
        cmd[0] = CMD_CONFIGURE_NMEA
        cmd[1] = gga & 0xff
        cmd[2] = gsa & 0xff
        cmd[3] = gsv & 0xff
        cmd[4] = gll & 0xff
        cmd[5] = rmc & 0xff
        cmd[6] = vtg & 0xff
        cmd[7] = zda & 0xff
        cmd[8] = attribute
        return self._sendRequest(cmd)

    def sendConfigureOutputMessageFormat(self, type_):
        if type_ not in (TYPE_NO_OUTPUT, TYPE_NMEA_MESSAGE, TYPE_BINARY_MESSAGE):
            type_ = TYPE_NMEA_MESSAGE
    
        cmd = bytearray(2)
        cmd[0] = CMD_CONFIGURE_OUTPUT_MESSAGE_FORMAT
        cmd[1] = type_
        return self._sendRequest(cmd)

    def sendConfigurePowerMode(self, mode, attribute):
        if mode not in (MODE_DISABLED, MODE_ENABLED):
            mode = MODE_ENABLED

        if attribute not in (ATTRIBUTE_UPDATE_TO_SRAM, ATTRIBUTE_UPDATE_TO_SRAM_AND_FLASH, ATTRIBUTE_TEMPORARY_ENABLED):
            attribute = ATTRIBUTE_UPDATE_TO_SRAM

        cmd = bytearray(3)
        cmd[ 0 ] = CMD_CONFIGURE_POWER_MODE
        cmd[ 1 ] = mode
        cmd[ 2 ] = attribute
        return self._sendRequest(cmd)

    def sendConfigurePositionUpdateRate(self, rate, attribute):
        if rate not in (RATE_1, RATE_2, RATE_4, RATE_5, RATE_8, RATE_10):
            rate = RATE_1

        if attribute not in (ATTRIBUTE_UPDATE_TO_SRAM, ATTRIBUTE_UPDATE_TO_SRAM_AND_FLASH):
            attribute = ATTRIBUTE_UPDATE_TO_SRAM

        cmd = bytearray(3)
        cmd[ 0 ] = CMD_CONFIGURE_POSITION_UPDATE_RATE
        cmd[ 1 ] = rate
        cmd[ 2 ] = attribute
        return self._sendRequest(cmd)

    def sendQueryPositionUpdateRate(self):
        cmd = bytearray(1)
        cmd[0] = CMD_QUERY_POSITION_UPDATE_RATE
        return self._sendRequest(cmd, RSP_POSITION_UPDATE_RATE)

    def sendConfigureDatum(self):
        # PENDING
        cmd = bytearray(19)
        cmd[0] = CMD_CONFIGURE_DATUM
        return self._sendRequest(cmd)
    
    def sendQueryDatum(self):
        cmd = bytearray(1)
        cmd[0] = CMD_QUERY_DATUM
        return self._sendRequest(cmd, RSP_GPS_DATUM)

    def sendGetEphemeris(self, svn):
        # don't accept all svn in one request (svn==0)
        if svn<1 or svn>32:
            svn=1
        cmd = bytearray(2)
        cmd[0] = CMD_GET_EPHEMERIS
        cmd[1] = svn
        return self._sendRequest(cmd, RSP_GPS_EPHEMERIS_DATA)

    def sendSetEphemeris(self):
        # PENDING
        cmd = bytearray(87)
        cmd[0] = CMD_SET_EPHEMERIS
        return self._sendRequest(cmd)
    
    def sendConfigureWAAS(self, enable, attribute):
        if attribute not in (ATTRIBUTE_UPDATE_TO_SRAM, ATTRIBUTE_UPDATE_TO_SRAM_AND_FLASH):
            attribute = ATTRIBUTE_UPDATE_TO_SRAM

        cmd = bytearray(3)
        cmd[ 0 ] = CMD_CONFIGURE_WAAS
        cmd[ 1 ] = enable & 0x01
        cmd[ 2 ] = attribute
        return self._sendRequest(cmd)

    def sendQueryWaasStatus(self):
        cmd = bytearray(1)
        cmd[0] = CMD_QUERY_WAAS_STATUS
        return self._sendRequest(cmd, RSP_GPS_WAAS_STATUS)

    def sendConfigurePositionPinning(self, enable):
        cmd = bytearray(2)
        cmd[ 0 ] = CMD_CONFIGURE_POSITION_PINNING
        cmd[ 1 ] = enable & 0x01
        return self._sendRequest(cmd)

    def sendQueryPositionPinning(self):
        cmd = bytearray(1)
        cmd[ 0 ] = CMD_QUERY_POSITION_PINNING
        return self._sendRequest(cmd, RSP_GPS_POSITION_PINNING_STATUS)

    def sendConfigurePositionPinningParameters(self):
        # PENDING
        cmd = bytearray(11)
        cmd[ 0 ] = CMD_CONFIGURE_POSITION_PINNING_PARAMETERS
        return self._sendRequest(cmd)

    def sendConfigureNavigationMode(self):
        # PENDING
        cmd = bytearray(3)
        cmd[ 0 ] = CMD_CONFIGURE_NAVIGATION_MODE
        return self._sendRequest(cmd)

    def sendQueryNavigationMode(self):
        cmd = bytearray(1)
        cmd[ 0 ] = CMD_QUERY_NAVIGATION_MODE
        return self._sendRequest(cmd, RSP_GPS_NAVIGATION_MODE)

    def sendConfigureOnePpsMode(self):
        # PENDING
        cmd = bytearray(3)
        cmd[ 0 ] = CMD_CONFIGURE_ONEPPS_MODE
        return self._sendRequest(cmd)

    def sendQueryOnePpsMode(self):
        cmd = bytearray(1)
        cmd[ 0 ] = CMD_QUERY_ONEPPS_MODE
        return self._sendRequest(cmd, RSP_GPS_ONEPPS_MODE)

    def sendLogStatusControl(self):
        cmd = bytearray(1)
        cmd[ 0 ] = CMD_LOG_STATUS_CONTROL
        return self._sendRequest(cmd, RSP_LOG_STATUS_OUTPUT)

    def sendLogConfigureControl(self, max_time, min_time, max_distance, min_distance, max_speed, min_speed, datalog_enable, mode_fifo):
        cmd = bytearray(27)
        cmd[0] = CMD_LOG_CONFIGURE_CONTROL
        cmd[1] = ((max_time >> 24) & 0x00ff)
        cmd[2] = ((max_time >> 16) & 0x00ff)
        cmd[3] = ((max_time >> 8) & 0x00ff)
        cmd[4] = (max_time & 0x00ff)
        cmd[5] = ((min_time >> 24) & 0x00ff)
        cmd[6] = ((min_time >> 16) & 0x00ff)
        cmd[7] = ((min_time >> 8) & 0x00ff)
        cmd[8] = (min_time & 0x00ff)
        cmd[9] = ((max_distance >> 24) & 0x00ff)
        cmd[10] = ((max_distance >> 16) & 0x00ff)
        cmd[11] = ((max_distance >> 8) & 0x00ff)
        cmd[12] = (max_distance & 0x00ff)
        cmd[13] = ((min_distance >> 24) & 0x00ff)
        cmd[14] = ((min_distance >> 16) & 0x00ff)
        cmd[15] = ((min_distance >> 8) & 0x00ff)
        cmd[16] = (min_distance & 0x00ff)
        cmd[17] = ((max_speed >> 24) & 0x00ff)
        cmd[18] = ((max_speed >> 16) & 0x00ff)
        cmd[19] = ((max_speed >> 8) & 0x00ff)
        cmd[20] = (max_speed & 0x00ff)
        cmd[21] = ((min_speed >> 24) & 0x00ff)
        cmd[22] = ((min_speed >> 16) & 0x00ff)
        cmd[23] = ((min_speed >> 8) & 0x00ff)
        cmd[24] = (min_speed & 0x00ff)
        cmd[25] = (datalog_enable & 0x01)
        cmd[26] = (mode_fifo & 0x01)
        return self._sendRequest(cmd)

    def sendLogClearControl(self):
        cmd = bytearray(1)
        cmd[0] = CMD_LOG_CLEAR_CONTROL
        return self._sendRequest(cmd)

    def sendLogSectorReadControl(self, sectorId):
        cmd = bytearray(2)
        cmd[0] = CMD_LOG_SECTOR_READ_CONTROL
        cmd[1] = sectorId & 0x00FF
        return self._sendRequest(cmd, RSP_SECTOR)

    def sendLogReadBatchControl(self, startSector, endSector):
        cmd = bytearray(5)
        cmd[0] = CMD_LOG_READ_BATCH_CONTROL
        cmd[1] = (startSector >> 8) & 0x00FF
        cmd[2] = startSector & 0x00FF
        cmd[3] = (endSector >> 8) & 0x00FF
        cmd[4] = endSector & 0x00FF
        return self._sendRequest(cmd, RSP_SECTOR)

    def _sendRequest(self, request, wait_for = 0):
        self.mutex.acquire()
        shared = type('ANONYMOUS', (object,), dict())
        shared.error = False
        shared.ack = False
        shared.nack = False
        shared.timeout = False
        shared.request = request
        shared.response = None
        shared.wait_for = wait_for
        if self.serialPort != None:
            checksum = 0
            fullRequest = bytearray([0xa0, 0xa1, (len(request) >> 8) & 0x00ff, len(request) & 0x00ff])
            fullRequest.extend(request)
            fullRequest.extend([reduce(operator.xor, request), 0x0d, 0x0a])
            try:
                self.shared = shared
                self.serialPort.write(str(fullRequest))
                start = time.time()
                while True:
                    if shared.nack:
                        break
                    if shared.ack :
                        if wait_for==0:
                            break
                        elif not shared.response is None:
                            break
                    if time.time() - start > self.timeout:
                        shared.timeout = True
                        break
            except:
                shared.error = True
                print '_sendRequest(): Write error'
            self.shared = None
        else:
            shared.error = True
        self.mutex.release()
        return shared

    def _portReader(self):
        while not self.abort.is_set():
            try:
                if self.serialPort.inWaiting() > 0:
                    b = self.serialPort.read(1)
                    if not self.serialData.append(b):
                        print '_portReader(): buffer overflow'
            except IOError:
                pass

    def _dispatcher(self):
        sectorReading = None
        sectorResponse = bytearray(4096 + 20)
        psr = 0

        binaryResponse = bytearray(2 + 2 + 65535 + 1 + 2) # 0xa0 0xa1 PL PAYLOAD CS 0x0d 0x0a
        pbr = 0

        nmeaResponse = bytearray(256)
        pnr = 0

        while not self.abort.is_set():
            b = self.serialData.pop()
            if b < 0:
                continue

            # Sector data: '\x??\x??....END\x00CHECKSUM=C\x00\x00\x0a\x0d', C: 1 byte checksum
            if sectorReading:
                sectorResponse[psr] = b
                psr = psr + 1
                if psr < len(sectorResponse):
                    if psr >= 18 and b == 0x0d:
                        if sectorResponse[psr - 18] == 0x45 and sectorResponse[psr - 17] == 0x4E and sectorResponse[psr - 16] == 0x44:
                            message = BinaryResponse.SectorResponse(sectorResponse[0:psr])
                            if message.crc_ok:
                                if message.valid:
                                    sectorReading.response = message
                                else:
                                    print '_dispatcher(): Invalid SECTOR response. Discarding %d bytes' % psr
                            else:
                                print '_dispatcher(): CRC Error in SECTOR response. Discarding %d bytes' % psr
                            sectorReading = None
                            psr = 0
                else:
                    print '_dispatcher(): long BINARY response. Discarding %d bytes' % psr
                    sectorReading = None
                    psr = 0
                continue

            # Binary response: '\xa0\xa1HL......C\x0d\x0a', C: 1 byte checksum
            if pbr == 0 and b == 0xa0:
                binaryResponse[pbr] = b
                pbr =  pbr + 1
                continue
            elif pbr:
                binaryResponse[pbr] = b
                pbr = pbr + 1
                if pbr==2:
                    if b != 0xa1:
                        print '_dispatcher(): wrong begin of binary response. Discarding %d bytes' % pbr
                        pbr=0
                elif pbr < len(binaryResponse):
                    if pbr < 4:
                        continue
                    elif pbr == 4:
                        last = 4 + binaryResponse[2] * 255 + binaryResponse[3] + 3
                    elif pbr == last:
                        if binaryResponse[pbr - 2] == 0x0d and binaryResponse[pbr - 1] == 0x0a:
                            message = BinaryResponse.BinaryResponse(binaryResponse[0:pbr])
                            if message.crc_ok:
                                if message.valid:
                                    _shared = self.shared
                                    if _shared is not None:
                                        if message.messageId == RSP_NACK:
                                            if message.ackId == _shared.request[0]:
                                                print '_dispatcher(): Expected NACK', Utils.sequence2Hex(list(message.message))
                                                _shared.nack = True
                                            else:
                                                print '_dispatcher(): unexpected NACK', Utils.sequence2Hex(list(message.message))
                                        elif message.messageId == RSP_ACK:
                                            if message.ackId == _shared.request[ 0 ]:
                                                #print '_dispatcher(): Expected ACK', Utils.sequence2Hex(list(message.message))
                                                _shared.ack = True
                                                if _shared.wait_for == RSP_SECTOR:
                                                    sectorReading = _shared
                                            else:
                                                print '_dispatcher(): unexpected ACK', Utils.sequence2Hex(list(message.message))
                                        elif message.messageId == _shared.wait_for and _shared.ack:
                                            #print '_dispatcher(): Expected BINARY response', Utils.sequence2Hex(list(message.message))
                                            _shared.response = message
                                        else:
                                            print '_dispatcher(): out of sync BINARY response. Discarding %d bytes' % pbr
                                    else:
                                        print '_dispatcher(): unexpected BINARY response. Discarding %d bytes' % pbr
                                    _shared = None
                                else:
                                    print '_dispatcher(): Invalid Binary response. Discarding %d bytes' % pbr
                            else:
                                print '_dispatcher(): CRC Error in Binary response. Discarding %d bytes' % pbr
                        else:
                            print '_dispatcher(): wrong end of Binary response. Discarding %d bytes' % pbr
                        pbr=0
                else:
                    print '_dispatcher(): long Binary response. Discarding %d bytes' % pbr
                    pbr = 0
                continue

            # NMEA response: '$GP???,..,...,*CS\x0d\x0a', CS: 2 ascii hex checksum
            if pnr == 0 and b == 0x24: # '$'
                nmeaResponse[pnr] = b
                pnr = pnr + 1
                continue
            elif pnr:
                nmeaResponse[pnr] = b
                pnr = pnr + 1
                if pnr < len(nmeaResponse):
                    if pnr > 5 and b == 0x0a:
                        if nmeaResponse[pnr - 5] == 0x2a and nmeaResponse[pnr - 2] == 0x0d:  # '*CS\x0d\x0a', CS: 2 ascii hex checksum
                            message = NmeaResponse.NmeaResponse(nmeaResponse[0:pnr])
                            if message.crc_ok:
                                if message.valid:
                                    self.nmea_observer(message)
                                else:
                                    print '_dispatcher(): Invalid NMEA response. Discarding %d bytes' % pnr
                            else:
                                print '_dispatcher(): CRC Error in NMEA response. Discarding %d bytes' % pnr
                        else:
                            print '_dispatcher(): wrong NMEA response. Discarding %d bytes' % pnr
                            print nmeaResponse[0:pnr]
                        pnr=0
                else:
                    print '_dispatcher(): long NMEA response. Discarding %d bytes' % pnr
                    pnr = 0
                continue

            # extra data
            print '_dispatcher(): extra data: ', b, chr(b)

    
if __name__ == '__main__':
    import time
    show_nmea=False
    
    def observer(message):
        if show_nmea:
            print message.message

    k=Skytraq()
    if k.connect('/dev/rfcomm0', 9600, 5, observer):
        #time.sleep(5)
        #show_nmea = False
        #r = k.sendQuerySoftwareVersion(SOFTWARE_TYPE_SYSTEM_CODE)
        #Utils.printObject('sendQuerySoftwareVersion', r.response)

        #r = k.sendQuerySoftwareCRC(SOFTWARE_TYPE_SYSTEM_CODE)
        #Utils.printObject('sendQuerySoftwareCRC', r.response)

        #r = k.sendQueryPositionUpdateRate()
        #Utils.printObject('sendQueryPositionUpdateRate', r.response)

        #r = k.sendGetEphemeris(0)
        #Utils.printObject('sendGetEphemeris', r.response)

        r = k.sendQueryDatum()
        Utils.printObject('sendQueryDatum', r.response)

        r = k.sendQueryWaasStatus()
        Utils.printObject('sendQueryWaasStatus', r.response)

        r = k.sendQueryPositionPinning()
        Utils.printObject('sendQueryPositionPinning', r.response)

        r = k.sendQueryNavigationMode()
        Utils.printObject('sendQueryNavigationMode', r.response)

        r = k.sendQueryOnePpsMode()
        Utils.printObject('sendQueryOnePpsMode', r.response)

        r = k.sendLogStatusControl()
        Utils.printObject('sendLogStatusControl', r.response)

        #k.sendSystemRestart(-1)
        #k.sendSetFactoryDefaults(-1)
        #k.sendConfigureSerialPort(-1, -1)
        #k.sendConfigureNmea(-1, -1, -1, -1, -1, -1, -1, -1)
        #k.sendConfigureOutputMessageFormat(-1)
        #k.sendConfigurePowerMode(-1, -1)
        #k.sendConfigurePositionUpdateRate(-1, -1)
        #k.sendConfigureDatum()
        #k.sendSetEphemeris()
        #k.sendConfigureWAAS(-1, -1)
        #k.sendConfigurePositionPinning(-1)
        #k.sendConfigurePositionPinningParameters()
        #k.sendConfigureNavigationMode()
        #k.sendConfigure1ppsMode()
        #sendLogConfigureControl(self, max_time, min_time, max_distance, min_distance, max_speed, min_speed, datalog_enable, mode_fifo):
        #sendLogClearControl(self):
        #sendLogSectorReadControl(self, sectorId):
        #sendLogReadBatchControl(self, startSector, endSector):

        time.sleep( 4 )
        k.disconnect()
    else:
        print 'Connect() error'
